#include <EEPROM.h>

#define encoder0PinA 20
#define encoder0PinB 21
#define encoder1PinA 2
#define encoder1PinB 3

#include <PID_v1.h>

#define AV LOW  // Marche avant
#define AR HIGH // Marche arrière

volatile long encoder0Pos = 0;  // "volatile" évite de copier la variable dans un registre temporaire
// et permet qu'elle soit bien modifiée à n'importe quel moment (interruptions)
volatile long encoder1Pos = 0;

volatile long newPositionG=0;
volatile long newPositionD=0;
// Sorties
int pinMoteurG = 9;
int pinMoteurD = 10;
int pinDirectionG = 11;
int pinDirectionD = 12;

//Entrées
const char pinTirette = 24;
unsigned char pin_button_arret = 36;

// Initialisations
const int echantillonage = 10; //delay echantillonage

int tempsActuel = 0;
int tempsAncien = 0;

int tensionMoteurG = 0;
int tensionMoteurSigneeG = 0;
int consigneG = 0;

int tensionMoteurD = 0;
int tensionMoteurSigneeD = 0;
int consigneD = 0;

float kp = 0.05;           // Coefficient proportionnel
float ki = 0.0012;           // Coefficient intégrateur
float kd = 0.00015;           // Coefficient dérivateur


double SetpointG, InputG, OutputG;
PID PID_G(&InputG, &OutputG, &SetpointG,kp,ki,kd, DIRECT);

double SetpointD, InputD, OutputD;
PID PID_D(&InputD, &OutputD, &SetpointD,kp,ki,kd, DIRECT);


void setup() {

  /* entrée boutton*/
  pinMode(pin_button_arret, INPUT); 
  digitalWrite(pin_button_arret, HIGH);/* on cable à l'intérieur du µcontroleur une résistance */
  Serial.begin(115200);

  pinMode(encoder0PinA, INPUT);
  pinMode(encoder0PinB, INPUT);
  pinMode(encoder1PinA, INPUT);
  pinMode(encoder1PinB, INPUT);

  pinMode(pinTirette, INPUT_PULLUP);  //initialise en entrée la pin de la tirette (avec resistance de pullup)
  //digitalWrite(encoder1PinA, HIGH);
  //digitalWrite(encoder1PinB, HIGH);

  attachInterrupt(3, doEncoderA, FALLING);
  attachInterrupt(4, doEncoderB, FALLING);
  attachInterrupt(0, doEncoderA2, FALLING);
  attachInterrupt(1, doEncoderB2, FALLING);

  pinMode(pinMoteurG,OUTPUT);
  pinMode(pinMoteurD,OUTPUT);
  pinMode(pinDirectionG,OUTPUT);
  pinMode(pinDirectionD,OUTPUT);

  digitalWrite(pinDirectionG, AV);
  digitalWrite(pinDirectionD, AV);

  newPositionG =0;
  newPositionD =0;

  InputG = newPositionG;
  SetpointG = consigneG;
  PID_G.SetOutputLimits(-255, 255);
  PID_G.SetMode(AUTOMATIC);
  PID_G.SetSampleTime(5000);

  InputD = newPositionD;
  SetpointD = consigneD;
  PID_D.SetOutputLimits(-255, 255);
  PID_D.SetMode(AUTOMATIC);
  PID_D.SetSampleTime(5000);

  Serial.println("\t\tProgramme acquisition parcourt");
  Serial.println("-Appuyez sur la touche 'a' pour demarrer l'acquisition"); 
  Serial.print("-Echantillonage a ");
  Serial.print(echantillonage);
  Serial.println("ms");
  Serial.println();
}

void loop()
{
  newPositionG = encoder0Pos/10;
  newPositionD = (encoder1Pos)/10;

  InputG = newPositionG;
  SetpointG = consigneG;
  PID_G.SetTunings(kp, ki, kd);
  PID_G.Compute();

  InputD = newPositionD;
  SetpointD = consigneD;
  PID_D.SetTunings(kp, ki, kd);
  PID_D.Compute();

  tempsActuel = millis();

  if (tempsActuel - tempsAncien == 100) // actualise la vitesse toutes les 100ms
  {
    tempsAncien = tempsActuel;


    // *****change la tension du moteur*****
    //tensionMoteurSigneeG += (consigneG - newPositionGGG)/100; //avec une accélération propostionnelle
    tensionMoteurSigneeG = OutputG;
    tensionMoteurSigneeD = OutputD;

    if(tensionMoteurSigneeG>0)
    {
      digitalWrite(pinDirectionG, AR);
    }
    else
    {
      digitalWrite(pinDirectionG, AV);
    }

    if(tensionMoteurSigneeD>0)
      digitalWrite(pinDirectionD, AR);
    else
      digitalWrite(pinDirectionD, AV);

    tensionMoteurG = abs(tensionMoteurSigneeG);
    if(tensionMoteurG>255)
      tensionMoteurG = 255;
    if(tensionMoteurG<0)
      tensionMoteurG = 0;

    tensionMoteurD = abs(tensionMoteurSigneeD);
    if(tensionMoteurD>255)
      tensionMoteurD = 255;
    if(tensionMoteurD<0)
      tensionMoteurD = 0;

    //constokjiorain(tensionMoteurG, 0, 255); // la valeur est reste entre 0 et 255

    // actualise les sorties
    analogWrite(pinMoteurG,tensionMoteurG);
    analogWrite(pinMoteurD,tensionMoteurD);
  }

  int index=Serial.read(); // lire un premier caractère

  if(index =='a') // si ce caractère est a
  {
    acquisition(pin_button_arret);
  }
}

void acquisition(unsigned char pin_button_arret)
{ 
  /* NOTE A LIRE */
 
  /* Constitution du tableau de l'EEPROM : 
    1) première case : numéro du parcourt
    2) 20 cases suivantes : indique le nmbre de positions enregistrées
    3) n cases suivantes indiquent les positions
        a) 1ère cases : position encodeur droit
        b) 2ème case : position encodeur gauche
    4) même shéma pour les parcourt suivants */
       
  char tableau_position_droite[10000]; /* en char parce que l'EEPROM est en char */
  char tableau_position_gauche[10000];
  
  int codeuse_position_droite[10000]; /* tableau qui contient toutes les positions des roues codeuses */
  int codeuse_position_gauche[10000];
  
  tableau_position_droite[0] = 0;
  tableau_position_gauche[0] = 0;
  
  codeuse_position_droite[0] = 0;
  codeuse_position_gauche[0] = 0;

  unsigned int nombre_potision_droite = 1; /* -1 pour toujours être dans le tableau lors du premier passage dans le "for" */
  unsigned int nombre_potision_gauche = 1;

  int i = 0, j=0;

  Serial.println("Acquisition en cours...\tBouton stop pour terminer");

  /* on rentre toutes les valeurs dans un tableau + on compte le nombre de positions*/
  while(digitalRead(pin_button_arret))
  {
    delay(echantillonage);
    codeuse_position_droite[nombre_potision_droite] = encoder0Pos;
    codeuse_position_gauche[nombre_potision_gauche] = encoder1Pos;

    /*on divise par 10 car une unité de l'EEPROM <=> 10 incréments */
    tableau_position_droite[nombre_potision_droite] = (encoder0Pos -  codeuse_position_droite[nombre_potision_droite -1])/10;
    tableau_position_gauche[nombre_potision_gauche] = (encoder1Pos -  codeuse_position_gauche[nombre_potision_gauche -1])/10;
    
    Serial.print((encoder0Pos -  codeuse_position_droite[nombre_potision_droite -1])/10);
    Serial.print("\t\t");
    Serial.println((encoder1Pos -  codeuse_position_gauche[nombre_potision_gauche -1])/10);
    
    nombre_potision_droite++;
    nombre_potision_gauche++;
  }
  
  //enregistrement dans l'EEPROM : mémoire de la chipkit
  /* enregistrement numéro parcourt */
  EEPROM.write(0, 1);
  
  /* enregistrement du nombre de positions */
  Serial.print("Affichage du nombre de positions ! : ");
  Serial.print(nombre_potision_droite, DEC);
  Serial.println(" positions.");

  for(int i = 1; i < 21; i++)
  { 
    if(((int)nombre_potision_droite - (255 * (i-1))) > 255) //si le nombre de position est > 255, on écrit 255
    {
      EEPROM.write(i, 255);       
    }
    else
    {
        if(((int)nombre_potision_droite - (255 * (i-1))) < 0) // si toutes les positions ont déjà été décomptés, on écrit 0
        {
          EEPROM.write(i, 0);       
        }
        else
        {
           EEPROM.write(i, ((int)nombre_potision_droite - (255 * (i-1)))%255); 
        }
    }
    Serial.print(EEPROM.read(i), DEC); Serial.print("\t");
  }
  
  Serial.println("Affichage de l'EEPROM !");
  j = 1;
  for(i = 21; i < nombre_potision_droite*2 + 20a; i+=2)
  {    
    EEPROM.write(i, tableau_position_droite[j]); 
    EEPROM.write(i+1, tableau_position_gauche[j]); 
    Serial.print(j, DEC);
    Serial.print(")\t");
    Serial.print(EEPROM.read(i), DEC); Serial.print("\t");
    Serial.println(EEPROM.read(i+1), DEC);
    j++;
  }
  
  Serial.print(nombre_potision_droite, DEC);
  Serial.println(" positions.");
  Serial.println("\r\nEEPROM terminer !");
  
  Serial.println("ALL DONE !!");

}


void doEncoderA(){

  // look for a low-to-high on channel A
  if (digitalRead(encoder0PinA) == HIGH) {

    // check channel B to see which way encoder is turning
    if (digitalRead(encoder0PinB) == LOW) {
      encoder0Pos = encoder0Pos + 1;         // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;         // CCW
    }
  }

  else   // must be a high-to-low edge on channel A
  {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder0PinB) == HIGH) {
      encoder0Pos = encoder0Pos + 1;          // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }
  }
  //Serial.println (encoder0Pos, DEC);
  // use for debugging - remember to comment out

}

void doEncoderB(){

  // look for a low-to-high on channel B
  if (digitalRead(encoder0PinB) == HIGH) {

    // check channel A to see which way encoder is turning
    if (digitalRead(encoder0PinA) == HIGH) {
      encoder0Pos = encoder0Pos + 1;         // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;         // CCW
    }
  }

  // Look for a high-to-low on channel B

  else {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder0PinA) == LOW) {
      encoder0Pos = encoder0Pos + 1;          // CW
    }
    else {
      encoder0Pos = encoder0Pos - 1;          // CCW
    }
  }

}
void doEncoderA2(){

  // look for a low-to-high on channel A
  if (digitalRead(encoder1PinA) == HIGH) {

    // check channel B to see which way encoder is turning
    if (digitalRead(encoder1PinB) == LOW) {
      encoder1Pos = encoder1Pos + 1;         // CW
    }
    else {
      encoder1Pos = encoder1Pos - 1;         // CCW
    }
  }

  else   // must be a high-to-low edge on channel A
  {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder1PinB) == HIGH) {
      encoder1Pos = encoder1Pos + 1;          // CW
    }
    else {
      encoder1Pos = encoder1Pos - 1;          // CCW
    }
  }
  //Serial.println (encoder0Pos, DEC);
  // use for debugging - remember to comment out

}

void doEncoderB2(){

  // look for a low-to-high on channel B
  if (digitalRead(encoder1PinB) == HIGH) {

    // check channel A to see which way encoder is turning
    if (digitalRead(encoder1PinA) == HIGH) {
      encoder1Pos = encoder1Pos + 1;         // CW
    }
    else {
      encoder1Pos = encoder1Pos - 1;         // CCW
    }
  }

  // Look for a high-to-low on channel B

  else {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoder1PinA) == LOW) {
      encoder1Pos = encoder1Pos + 1;          // CW
    }
    else {
      encoder1Pos = encoder1Pos - 1;          // CCW
    }
  }

}



